#include "p24HJ128GP504.h"
#include "adc_algs.h"
#include "adc_sensors.h"
#include "stepper.h"

extern AdcBuf adcBufA;
extern AdcBuf adcBufB;

extern unsigned int *modeCompressionPosition;
extern unsigned int *modeReboundPosition;
extern unsigned char impactDbg;

//This buffer size determines our window size. Each sample is approximately
//taken every .002s. Therefore 128 corresponds to approximately 1/4 second.

unsigned int windowImpact[BUFF_SIZE] = {0};
unsigned int windowPot[BUFF_SIZE] = {0}; 
unsigned int windowAngle[BUFF_SIZE] = {0};
unsigned char angleNegativeFlag[BUFF_SIZE] = {0};  //1 is positive, 0 is negative 

unsigned int windowPoint = 0;

unsigned int peakBuffer[12] = {0}; //the size is determined by 3 seconds. Each block corresonds to one 1/4s window.
unsigned int peakCount = 0;

unsigned int angleBuffer[4] = {0}; //the size is determined by 1 second. Each block corresonds to one 1/4s window.
unsigned int angleCount = 0;

unsigned char lockoutFlag = 0; //0 indicates not in lockout, 1 indicates in lockout
unsigned char washBoardFlag = 0;

unsigned int diff2Angle[] = {1, 1, 1, 2, 2, 2, 3, 3, 3, 4,
                             4, 4, 5, 5, 5, 6, 6, 6, 7, 7,
                             7, 8, 8, 8, 9, 9, 9, 10, 10, 10,
                             11, 11, 11, 12, 12, 12, 13, 13, 13, 14,
                             14, 14, 15, 15, 15, 16, 16, 16, 17, 17,
                             17, 18, 18, 18, 19, 19, 19, 20, 20, 20,
                             21, 21, 21, 22, 22, 22, 23, 23, 23, 24,
                             24, 24, 25, 25, 25, 26, 26, 26, 27, 27,
                             27, 28, 28, 28, 29, 29, 29, 30, 30, 30,
                             31, 31, 31, 32, 32, 32, 33, 33, 33, 34,
                             34, 34, 35, 35, 35, 36, 36, 36, 37, 37,
                             37, 38, 38, 38, 39, 39, 39, 40, 40, 40,
                             41, 41, 41, 42, 42, 42, 43, 43, 43, 44,
                             44, 44, 45, 45, 46, 46, 46, 47, 47, 47,
                             48, 48, 48, 49, 49, 49, 50, 50, 50, 51,
                             51, 51, 52, 52, 52, 53, 53, 53, 54, 54,
                             54, 55, 55, 55, 56, 56, 57, 57, 57, 58,
                             58, 58, 59, 59, 59, 60, 60, 60, 61, 61,
                             61, 62, 62, 62, 63, 63, 63, 64, 64, 64,
                             65, 65, 65, 66, 66, 66, 67, 67, 67, 68,
                             68, 68, 69, 69, 69, 70, 70, 70, 71, 71, 
                             71, 72, 72, 72, 73, 73, 73, 74, 74, 74, 
                             75, 75, 75, 76, 76, 76, 77, 77, 77, 78, 
                             78, 78, 79, 79, 79, 80, 80, 80, 81, 81,
                             82, 82, 82, 83, 83, 83, 84, 84, 84, 85,
                             85, 85, 86, 86, 87, 87, 88, 88, 88, 89,
                             89, 89, 90, 90, 90, 90, 90, 90, 90, 90,
                             90, 90, 90, 90, 90, 90, 90, 90, 90, 90,
                             90 };



void lockoutDetect(void)
{
    unsigned int i = 0;
    unsigned int j = 0; 
    unsigned int angleSum1 = 0;
    unsigned int angleSum2 = 0;
    
    for(i = 0; i<BUFF_SIZE; i++)
    {
	    angleSum1 += windowAngle[i]*angleNegativeFlag[i];
        //sum the angles for the last 1/4s
    }
    

    if ((angleSum1/BUFF_SIZE)>=(7))
    {
        angleBuffer[angleCount] = 1;
        //if the average angle over the last 1/4s is > 15 degrees indicate this with a 1 in the angleAvg array
    }
    else
    {
        angleBuffer[angleCount] = 0;
    }     

    angleCount++;
    if(angleCount>=4)
    {
        angleCount=0;
    }    

    for(j=0; j<4;j++)
    {    
        angleSum2 += angleBuffer[j];
        //sum the average angles for the last 4 1/4s windows
    }
    if(angleSum2>=4)
    {
        //if over the last second the bike has continuously been on an angle > 15 we go into lockout
        lockout();   
    }
}

void lockout(void)
{
    //Send Compression Motor to 100% and leave Rebound alone; 
    lockoutFlag = 1;
    setCompressionPosition(LOCKOUT);   
}



void breakoutDetect(void)
{
    #define BREAKOUT_THRESH 725
    unsigned int i;
    unsigned int start = 0;
    
    if(windowPoint >= 32 )
    {
        start = windowPoint - 32;
    }    
    for( i = 0; i < 512; i++ )
    {
        if ( windowImpact[i] >= BREAKOUT_THRESH )
        {
            exitLockout();
        }
    }
    for( i = 0; i < 4; i++ )
    {
        angleBuffer[i] = 0;   
    }    
}


void exitLockout(void)
{
    //Return Dampers to Selected Ride Mode;
    LATCbits.LATC6 ^= 1;    
    lockoutFlag = 0;
    setCompressionPosition( *modeCompressionPosition ); 
}



void peakDetect(void)
{
    unsigned int i = 0;
    #define PEAK_THRESH  732   //corresponds to 45mm of travel

    peakBuffer[peakCount] = 0;

    for (i=1;i<BUFF_SIZE;i++)
    {
        if((windowPot[i-1]<=PEAK_THRESH) && (windowPot[i]>=PEAK_THRESH))
        {
            peakBuffer[peakCount] = 1;
            break;
            //break out of for loop if we detect a peak above threshold
        }
        
    }
    

    peakCount++;
    if (peakCount >=12)
    {
       peakCount = 0;
    }

   
    if( (washboardDetect()) == 1 )
        return;
    else if( washBoardFlag == 1 )
    {    
        setCompressionPosition(*modeCompressionPosition);
        setReboundPosition(*modeReboundPosition);
        //after we have scanned the current 1/4s window, scan the last 3 seconds for multiple peaks
        washBoardFlag = 0;
    }    
}


unsigned char washboardDetect(void)
{
    unsigned int i = 0; 
    unsigned int peakSum = 0; 
    #define WASHTHRESH  4 

    for(i=0; i<12; i++)
    {
        peakSum += peakBuffer[i];
        //sum the peak detection buffer over the last 3 seconds
    }


    if (peakSum >= WASHTHRESH)
    {
        //if there are more than washThreshold number of peaks in our buffer
        //enter into washboard mode-----> decrease rebound and compression
        if( washBoardFlag == 0 )
        {
            setCompressionPosition(66);
            setReboundPosition(480);
            washBoardFlag = 1;
        }        
        return 1;
    }
    return 0;
}  

unsigned char angleConv(unsigned int X, unsigned int Y, unsigned char i)
{
    unsigned int tempdiff = 0; 

    if ((X>=636) && (Y>=644)) 
    {
        angleNegativeFlag[i+windowPoint] = 0; 
        return 28;
    }
    else
    if ((X>=636) && (Y<644))
    {
        X = 644;
        tempdiff = (X-Y);
        tempdiff = diff2Angle[tempdiff];
        
        if( tempdiff < 28 )
        {
            angleNegativeFlag[i+windowPoint] = 0;
            return 28 - tempdiff;
        }    
        
        angleNegativeFlag[i+windowPoint] = 1; 
        return (unsigned char)tempdiff - 28;
    }
    else
    if ((Y>=644) && (X<636))
    {
        Y = 644; 
        tempdiff = (Y-X);
        tempdiff = diff2Angle[tempdiff];
        
        if( tempdiff >= 62 )
        {
            angleNegativeFlag[i+windowPoint] = 1;
            return (unsigned char)(152 - tempdiff);
        }    
        
        angleNegativeFlag[i+windowPoint] = 0; 
        return (unsigned char)tempdiff;
    }
    
    // Assume that if neither case is true it must be zero
    // Both X and Y can never both be less than 636
    return 0;
}  
